/*
 *  run.cpp
 *  FaceTracker
 *
 *  Created by Kenn Sebesta on 2007-11-22.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "ballFinder.h"
#include "cameraSetup.h"
#include "imageCropper.h"
#include "run.h"
#include "updateTrajectory.h"
#include <OpenCV/OpenCV.h>
#include <iostream>
#include <string>
#include <sstream>
using namespace std;

const char  *WINDOW_RUN  = "Ball and Plate";
const char  *WINDOW_ADJUST  = "Parameters";
char tbarname1[] = "Upper Threshold";
char tbarname2[] = "Lower Threshold";

void PrintMat( CvMat * );
void imagesc( CvMat * );
int run()
{
	
	cvNamedWindow( WINDOW_RUN, CV_WINDOW_AUTOSIZE );
	cvNamedWindow( WINDOW_ADJUST, CV_WINDOW_AUTOSIZE );
	cvMoveWindow( WINDOW_RUN, 300, 300);
 //------------------------------SANDBOX-------------------------//

	/*
	const char *pathName;
	
	CvFileStorage* userVariables = cvOpenFileStorage( "test.yml", 0, CV_STORAGE_READ);	
	pathName = cvReadStringByName( userVariables, NULL, "ballPath");
	std::cout << "pathName is  " << pathName << "\n";

	// Local Garbage collection
	cvReleaseFileStorage( &userVariables );
	std::cout << "pathName is  " << pathName << "\n";
	*/
	
/*	pid_t pid;
	int rv;
	char s[300];
	int num, fd;
	#define FIFO_NAME "american_maid"
	
	switch(pid=fork()) {
		case -1:
			perror("fork");  
			exit(1);         
			
		case 0:

			mknod(FIFO_NAME, S_IFIFO | 0666, 0);
			
			printf("waiting for writers...\n");
			fd = open(FIFO_NAME, O_RDONLY);
			printf("got a writer:\n");
			
			do {
            if ((num = read(fd, s, 300)) == -1)
					perror("read");
            else {
					s[num] = '\0';
					printf("tick: read %d bytes: \"%s\"\n", num, s);
            }
			} while (num > 0);
			
		default:
			

			mknod(FIFO_NAME, S_IFIFO | 0666, 0);
			
			printf("waiting for readers...\n");
			fd = open(FIFO_NAME, O_WRONLY);
			printf("got a reader--type some stuff\n");
			
			while (gets(s), !feof(stdin)) {
            if ((num = write(fd, s, strlen(s))) == -1)
					perror("write");
            else
					printf("speak: wrote %d bytes\n", num);
			}
	}
	


	exit(1);*/
 	
	//------------------------------SANDBOX-------------------------//
	
	//==============================================
	// Load config file
	//==============================================
	double delT, ballSpeedR;
	CvKalman *kalman_dscrt = cvCreateKalman(4,2,2);
	char *pathName;

	CvFileStorage* userVariables = cvOpenFileStorage( "bandpUserVariables.yml", 0, CV_STORAGE_READ);	
	delT = cvReadRealByName( userVariables, NULL, "delT");
	ballSpeedR = cvReadRealByName( userVariables, NULL, "ballSpeedR");
	pathName = (char*)cvReadStringByName( userVariables, NULL, "pathName");
	kalman_dscrt->process_noise_cov=(CvMat*)cvReadByName( userVariables, NULL, "Q_kalman" );
	kalman_dscrt->measurement_noise_cov=(CvMat*)cvReadByName( userVariables, NULL, "R_kalman" );
	
	pathName="circleChase"; // This doesn't seem to work for some reason in cvReadStringByName


	//DOESN'T WORK YET
	double sat=.05;
	CvMat *tableCenter, *setPoint, *setPointR;
	tableCenter=cvCreateMat( 1, 2, CV_32F );
		cvmSet( tableCenter, 0, 0, 740 ); cvmSet( tableCenter, 0, 1, 440 );
	setPoint=cvCreateMat( 4, 1, CV_32F );
	setPointR=cvCreateMat( 4, 1, CV_32F );
	
	// Local Garbage collection
	cvReleaseFileStorage( &userVariables );

	printf("Done with file read.\n");

	
	//---------------------------------------------------------
	/*
#define FIFO_NAME "american_maid"
	
	char s[300];
	int num, fd;
	
	mknod(FIFO_NAME, S_IFIFO | 0666, 0);
	
	printf("waiting for readers...\n");
	//	fd = open(FIFO_NAME, O_WRONLY);
	printf("got a reader--type some stuff\n");
	
	std::cout<< *tableCenter.type << std::endl;
//	sprintf( s, "%d", kalman_dscrt->process_noise_cov->data);
//	write(fd, s, sizeof(&kalman_dscrt->process_noise_cov->data));
	
	
	exit(1);
	 */
	//---------------------------------------------------------
	
	//==============================================
	// Load generated data file
	//==============================================
	IplImage *maskImage;
	CvMat *K_dlqr;
	
	int threshHigh, threshLow;
	double calibration;
	
	CvFileStorage* systemVariables = cvOpenFileStorage( "bandpSystemVariables.yml", 0, CV_STORAGE_READ);
	threshHigh = cvReadIntByName ( systemVariables, NULL, "threshHigh");
	threshLow = cvReadIntByName ( systemVariables, NULL, "threshLow");
	calibration = cvReadRealByName ( systemVariables, NULL, "calibration");
	kalman_dscrt->transition_matrix=(CvMat*)cvReadByName( systemVariables, NULL, "A_dscrt" );
	kalman_dscrt->control_matrix=(CvMat*)cvReadByName( systemVariables, NULL, "B_dscrt" );
	kalman_dscrt->measurement_matrix=(CvMat*)cvReadByName( systemVariables, NULL, "C" );
	kalman_dscrt->state_post=(CvMat*)cvReadByName( systemVariables, NULL, "x_hatR" );
	kalman_dscrt->error_cov_post=(CvMat*)cvReadByName( systemVariables, NULL, "PkCovMat" );
	K_dlqr=(CvMat*)cvReadByName( systemVariables, NULL, "K_dlqr" );
	maskImage=(IplImage*)cvReadByName( systemVariables, NULL, "maskImage" );	
	
	// Check hash
	//NEEDS TO BE WRITTEN.

	// Stupidity check
	std::cout << "pathName is  " << pathName << "\n";
	std::cout << "threshHigh is  " << threshHigh << "\n";
	std::cout << "threshLow is  " << threshLow << "\n";
	std::cout << "calibration is  " << calibration << "\n";
	PrintMat( kalman_dscrt->transition_matrix );
	PrintMat( kalman_dscrt->control_matrix );
	PrintMat( kalman_dscrt->measurement_matrix );
	PrintMat( kalman_dscrt->state_post );
	PrintMat( kalman_dscrt->process_noise_cov );
	PrintMat( kalman_dscrt->measurement_noise_cov );
	PrintMat( kalman_dscrt->error_cov_post );
	PrintMat( K_dlqr );
//	cvShowImage( WINDOW_RUN, maskImage );
	
	// Local Garbage collection
	cvReleaseFileStorage( &systemVariables );

	printf("Done with file read.\n");	

	
	//==============================================
	// Open COM port
	//==============================================

	//OS dependent.  #include <SerialStream.h> from libserial
	
	
	//==============================================
	// Set up camera
	//==============================================
	CvCapture *camFeed = cameraSetup( );
	IplImage *camFrame = cvQueryFrame( camFeed ); //DO NOT RELEASE THIS IMAGE
	if ( !camFrame ) {
		fprintf(stderr, "ERROR: camFrame is null. Exiting...\n");
		exit (1);
	}
	//This module justs lets me test my program without a camera being attached.
	for (int i=1; i < 30 ; i++)
	{
//		camFrame = cvQueryFrame( camFeed );
//		cvShowImage( WINDOW_RUN, camFrame );
	}
	
	printf( "Camera initialized.\n" );

	
	//==============================================
	// Initialize loop matrices and variables
	//==============================================
	CvMat *x_hat=cvCreateMat( 4, 1, CV_32F );
	CvMat *x_hatR=cvCreateMat( 4, 1, CV_32F );
	CvMat *y_measrd=cvCreateMat( 2, 1, CV_32F );
	CvMat *y_measrdR=cvCreateMat(2, 1, CV_32F );
	CvMat *u=cvCreateMat( 2, 1, CV_32F );
	CvMat *errMat=cvCreateMat( 4, 1, CV_32F );
	CvMat *integrationErr=cvCreateMat( 4, 1, CV_32F );	cvZero( integrationErr );
	CvRect cropRect=cvRect(0,0,0,0);
	CvRect limitR=cvRect( 0, 0, camFrame->width, camFrame->height );

	double minConv, maxConv, tmpSat;
	double ticRate = cvGetTickFrequency()*1e06;
	double *convMin, *convMax;
		convMax=&maxConv; convMin=&minConv;//Necessary to assign pointer, or else things crash.	
	int keyPress=-1, keyHold='1', ballGone=1, missingCount=0, nthLoop=0;
	int64 tic_speed, tic_fps, toc;
	char* filename = "./frame.bmp"; //filename for saving image
	int viewSize=round( maskImage->width+2*0.20*calibration*0.04 ); // viewSize is the viewport size, where 0.20 is the maximum speed in m/s that the ball will likely reach.
	double ballSpeed=ballSpeedR*calibration/delT;
	
	CvFont textFont;
	//	cvInitFont( textFont, 
	cvInitFont( &textFont, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.6, 0, 1);
	char *ballLocText = new char[13];
	
	cvCreateTrackbar(tbarname1, WINDOW_ADJUST, &threshHigh, 300, NULL );
	cvCreateTrackbar(tbarname2, WINDOW_ADJUST, &threshLow, 50, NULL );

	
	printf( "Variables initialized.\n" );
	
	//=====================================================================================
	// MAIN LOOP
	//=====================================================================================
	printf( "Program starting...\n" );
	while( keyPress != 27 )
	{	
		nthLoop++; // Keeps track of time
		tic_fps=cvGetTickCount();
		camFrame = cvQueryFrame( camFeed );
		cvSaveImage(filename, camFrame); // Having crashing problem, this way I can always know what image crashed it.
		tic_speed=cvGetTickCount();
		//==============================================
		// Imaage analysis module that finds the ball
		//==============================================

		IplImage *bwImageOUT, *edgeImageOUT;
		CvMat *convMatOUT;
		// Creates image cropping rectangle so the loop runs faster
		imageCropper( &cropRect, y_measrd, limitR, viewSize, missingCount, ballGone ); 
		// Image analysis module that locates the ball.
		ballFinder( maskImage, camFrame, cropRect, threshLow, threshHigh,	//Inputs
					  convMin, convMax, y_measrd, &convMatOUT, &edgeImageOUT, &bwImageOUT );	//Outputs

		//Have we found the ball?
		if ( maxConv>3.5e06){ //This needs to be modularized. Right now, this value needs to be changed here AND in ballFinder.cpp
			missingCount=0;
			ballGone=0;
//			std::cout << "Max conv. value: " << maxConv <<  std::endl; //Just a check so I know what the max value is for normal operation.
		}
		else{
			missingCount++;
			if ( missingCount > (int)(0.25/delT) )
				ballGone=1;
			std::cout << "Missing ball for " << missingCount << " frames. Max conv. value: " << maxConv << ".  BallGone: " << ballGone << std::endl;
		}

		cvScale( y_measrd, y_measrdR, 1/calibration );
		
		
		//==============================================
		// Observer module that reconstructs the ball's entire state
		//==============================================
		cvKalmanPredict( kalman_dscrt, u ); //This line should be before the image analysis module.
		cvKalmanCorrect( kalman_dscrt, y_measrdR );
		x_hatR=kalman_dscrt->state_post;
		cvScale( x_hatR, x_hat, calibration );
		
		//==============================================
		// Trajectory module that generates the set point
		//==============================================
		updateTrajectory( (char*)pathName, nthLoop, ballSpeed, &integrationErr, &setPoint, tableCenter );
		
		//==============================================
		// Controller module that generates the table's inputs
		//==============================================
		cvScale( setPoint, setPointR, 1/calibration );
		cvSub( setPointR, x_hatR, errMat );
		cvMatMul( K_dlqr, errMat, u );
		
		// Saturation
		tmpSat=cvmGet( u, 0, 0 );
		if ( (tmpSat > sat) || (tmpSat < -sat) ){
			if (tmpSat > 0)
				cvmSet( u, 0, 0, sat);
			else
				cvmSet( u, 0, 0, -sat);
		}	
		tmpSat=cvmGet( u, 1, 0 );
		if ( (tmpSat > sat) || (tmpSat < -sat) ){
			if (tmpSat > 0)
				cvmSet( u, 1, 0, sat);
			else
				cvmSet( u, 1, 0, -sat);
			}	

		//==============================================
		// cntrOutput(); // Module that outputs the control signal to the actuators
		// This routine MUST shut itself down if we can't find the ball.
		//==============================================
	
		if (ballGone == 0 ){
			//SEND SOME STUFF TO ACTUATORS.
		}
		
		//==============================================
		// dsplyOutput(); //
		// Display image by pressing '1'
		// Display binarised image by pressing '2'
		// Display edge extraction image by pressing '3'
		// Display convolution image by pressing '4'
		// Save image by pressing 's'
		//==============================================
		if( 1 ){
		printf("u= %4.4f %4.4f , ", cvmGet( u, 0, 0 ), cvmGet( u, 1, 0 ));
			printf("x_hatR= %4.4f %4.4f %4.4f %4.4f  \n", cvmGet( x_hatR, 1, 0 ), cvmGet( x_hatR, 1, 0 ), cvmGet( x_hatR, 1, 0 ), cvmGet( x_hatR, 1, 0 ));
		printf("  y_measrd= %4.4f %4.4f , ", cvmGet( y_measrd, 0, 0 ), cvmGet( y_measrd, 1, 0 ));
			printf("Set point = %5.1f x %5.1f\n", cvmGet(setPoint, 0, 0), cvmGet(setPoint, 2, 0) );
		}

		if ( keyHold=='1' ){
			sprintf(ballLocText, "%3.0f x %3.0f", round(cvmGet(y_measrd, 0, 0)), round(cvmGet(y_measrd, 1, 0)));
			cvPutText( camFrame, ballLocText, cvPoint( cropRect.x, cropRect.y-3 ), &textFont , cvScalar(255, 255, 255) );
			cvRectangle( camFrame, cvPoint( cropRect.x, cropRect.y ), cvPoint( cropRect.x+cropRect.width, cropRect.y+cropRect.height ), cvScalar(255, 255, 255), 1);
			cvCircle( camFrame, cvPoint(round(cvmGet(y_measrd, 0, 0)), round(cvmGet(y_measrd, 1, 0))), (maskImage->width-1)/2, cvScalar(255, 255, 255), 1 );	
			cvCircle( camFrame, cvPoint(round(cvmGet(x_hat, 0, 0)), round(cvmGet(x_hat, 2, 0))), 5, cvScalar(200, 200, 200), 3 );	
			cvCircle( camFrame, cvPoint(round(cvmGet(setPoint, 0, 0)), round(cvmGet(setPoint, 2, 0))), 2, cvScalar(255, 255, 255), 1 );	
			cvShowImage( WINDOW_RUN, camFrame );
		}
		else if ( keyHold=='2' ){
			cvShowImage( WINDOW_RUN, bwImageOUT );
		}
		else if ( keyHold=='3' ){
			cvShowImage( WINDOW_RUN, edgeImageOUT );
		}
		else if ( keyHold=='4' ){
			if ( std::max(convMatOUT->cols,convMatOUT->rows) < 100 ) // imagesc is too slow, so have to limit its use.
				imagesc(convMatOUT);
		}
		else if( keyPress == 's'){
			cvSaveImage(filename, camFrame), printf( "Image saved\n" );
		}
		
		// Loop garbage collection
		cvReleaseMat( &convMatOUT );
		cvReleaseImage( &bwImageOUT );
		cvReleaseImage( &edgeImageOUT );

		//====================================
		// Exit program by pressing ESC key.
		//====================================
		keyPress = cvWaitKey(5);
		if (keyPress !=-1)
			keyHold=keyPress;		
		
		//Output time
		toc = cvGetTickCount();
		printf("    Computation time: %3.3lf ms. FPS: %lf fps\n", (double)(toc-tic_speed)*1000/ticRate, ticRate/(double)(toc-tic_fps) );
		
	}
	
 	//=====================================================================================
	// Garbage collection
	//=====================================================================================
	
	cvReleaseImage( &maskImage ); 
	cvDestroyWindow( WINDOW_RUN );
	cvReleaseMat( &x_hat );
	cvReleaseMat( &x_hatR );
	cvReleaseMat( &y_measrd );
	cvReleaseMat( &y_measrdR );
	cvReleaseMat( &u );
	cvReleaseCapture( &camFeed );

	return 1;
}


void imagesc( CvMat *inputImage )
{
	
	// This is an attempt at remaking the matlab 'imagesc' function. The only colormap right now is 'jet'.
	IplImage *colorMapJet;
	colorMapJet=cvCreateImage( cvSize(inputImage->cols, inputImage->rows), IPL_DEPTH_8U, 3);
	double min, max;
	
	cvMinMaxLoc(inputImage, &min, &max, NULL, NULL, NULL);
	
	double scale=255/(max-min);
	cvScale( inputImage, inputImage, scale, 0 ); //Normalizes matrix to 0-255 (grayscale)

	int k;
	for (int i=0; i<inputImage->cols; i++){
		for (int j=0; j<inputImage->rows; j++){
			k=(int)cvGetReal2D( inputImage, j, i );			
			if (k==0){
				cvSet2D( colorMapJet, j, i, cvScalar( 0, 0, 0 ));
			}
			if ( k< 51){
				cvSet2D( colorMapJet, j, i, cvScalar( 200+i, (k)*5, 0 ));
			}
			else if ( k< 101){
				cvSet2D( colorMapJet, j, i, cvScalar( 255-(k-50)*5, 255, 0 ));
			}
			else if ( k< 151){
				cvSet2D( colorMapJet, j, i, cvScalar( 0, 255, (k-100)*5 ) );
			}
			else if ( k< 201){
				cvSet2D( colorMapJet, j, i, cvScalar( 0, 255-(k-150)*5, 255 ));
			}
			else{
				cvSet2D( colorMapJet, j, i, cvScalar( 0, 0, 255-(k-200)*3 ));
			}
		}
	}
									
	cvShowImage(WINDOW_RUN, colorMapJet);
	cvReleaseImage(&colorMapJet);
	
	return;
}